﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.IO;
using MotionPlanning.MotionPlanningComponents.MotionPlanningSystems;

namespace MotionPlanning.MotionPlanningComponents
{
    public class SamplingComponent
    {
        public enum SamplingMode
        {
            SAMPLING,
            CONTROL,
            SELECT,
            SHOOTING
        }

        private Random Rand_sampling;
        private Random Rand_control;
        private Random Rand_select;
        private Random Rand_shooting;
        private int Seed_sampling;
        private int Seed_control;
        private int Seed_select;
        private int Seed_shooting;

        override public string ToString()
        {
            string content = "";
            content += "\n\t<Seed_sampling>" + Seed_sampling + "</Seed_sampling>";
            content += "\n\t<Seed_control>" + Seed_control + "</Seed_control>";
            content += "\n\t<Seed_select>" + Seed_select + "</Seed_select>";
            content += "\n\t<Seed_shooting>" + Seed_shooting + "</Seed_shooting>\n";
            return content;
        }
        public double[] NextQ(SamplingMode mode, RoboticSystemComponent system)
        {
            Random randomGenerator = null;
            if (mode == SamplingMode.SAMPLING)
            {
                randomGenerator = Rand_sampling;
            }
            else if (mode == SamplingMode.SELECT)
            {
                randomGenerator = Rand_select;
            }
            else
            {
                throw new Exception("unknown random generator selected in NextQ, MODE= " + mode);
            }
            if (system == null)
            {
                throw new Exception("system is null in NextQ");
            }
            int DOF = system.Get_DOF_state();
            double[] high = system.Get_q_max();
            double[] low = system.Get_q_min();
            double[] v = new double[DOF];
            lock(randomGenerator)
            {
                for (int i = 0; i < DOF; ++i)
                    v[i] = randomGenerator.NextDouble() * (high[i] - low[i]) + low[i];
            }
            return v;
        }
        public double[] NextControl(SamplingMode mode, RoboticSystemComponent system)
        {
            /**
             * low.length= DOF+1
             * high.length= DOF+1
             * returned double[].length=DOF+1
             */
            Random randomGenerator = null;
            if (mode == SamplingMode.CONTROL)
            {
                randomGenerator = Rand_control;
            }
            else if (mode == SamplingMode.SHOOTING)
            {
                randomGenerator = Rand_shooting;
            }
            else
            {
                throw new Exception("unknown random generator selected in NextControl, MODE= " + mode);
            }
            if (system == null)
            {
                throw new Exception("system is null in NextControl");
            }
            int DOF = system.Get_DOF_control();
            double[] high = system.Get_ctrl_max();
            double[] low = system.Get_ctrl_min();
            double[] v = new double[DOF + 1];
            lock (randomGenerator)
            {
                for (int i = 0; i <= DOF; ++i)
                    v[i] = randomGenerator.NextDouble() * (high[i] - low[i]) + low[i];
            }
            return v;
        }
        public void ResetSeed_sampling(int seed)
        {
            Seed_sampling = seed;
            Rand_sampling = new Random(Seed_sampling);
        }
        public void ResetSeed_control(int seed)
        {
            Seed_control = seed;
            Rand_control = new Random(Seed_control);
        }
        public void ResetSeed_select(int seed)
        {
            Seed_select = seed;
            Rand_select = new Random(Seed_select);
        }
        public void ResetSeed_shooting(int seed)
        {
            Seed_shooting = seed;
            Rand_shooting = new Random(Seed_shooting);
        }
        public int GetSeed_sampling() { return Seed_sampling; }
        public int GetSeed_control() { return Seed_control; }
        public int GetSeed_select() { return Seed_select; }
        public int GetSeed_shooting() { return Seed_shooting; }
        public SamplingComponent()
        {
            ResetSeed_sampling((int)DateTime.Now.Ticks);
            ResetSeed_control((int)DateTime.Now.Ticks * 2);
            ResetSeed_select((int)DateTime.Now.Ticks * 3);
            ResetSeed_shooting((int)DateTime.Now.Ticks * 4);
        }
        public SamplingComponent(int seed_sampling, int seed_control, int seed_select, int seed_shooting)
        {
            ResetSeed_sampling(seed_sampling);
            ResetSeed_control(seed_control);
            ResetSeed_select(seed_select);
            ResetSeed_shooting(seed_shooting);
        }
        public SamplingComponent(string fileInput)
        {
            if (fileInput == null)
                throw new Exception("SamplingComponent fileInput cannot be null");

            string[] separators = new string[] { "<Seed_sampling>", "</Seed_sampling>" };
            string[] entries = fileInput.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3)
                throw new Exception("SamplingComponent fileInput is invalid");
            ResetSeed_sampling(int.Parse(entries[1]));

            separators = new string[] { "<Seed_control>", "</Seed_control>" };
            entries = fileInput.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3)
                throw new Exception("SamplingComponent fileInput is invalid");
            ResetSeed_control(int.Parse(entries[1]));

            separators = new string[] { "<Seed_select>", "</Seed_select>" };
            entries = fileInput.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3)
                throw new Exception("SamplingComponent fileInput is invalid");
            ResetSeed_select(int.Parse(entries[1]));

            separators = new string[] { "<Seed_shooting>", "</Seed_shooting>" };
            entries = fileInput.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3)
                throw new Exception("SamplingComponent fileInput is invalid");
            ResetSeed_shooting(int.Parse(entries[1]));
        }

        //refactorize this function
        Random Rand_noise;
        public void NextNoisyControl(   RoboticSystemComponent system,
                                        double amplitude_noise,
                                        double duration_max, 
                                        ref double[] noisyControl   )
        {
            int DOF = system.Get_DOF_control();
            double[] high = system.Get_ctrl_max();
            double[] low = system.Get_ctrl_min();
            if(noisyControl==null)
                noisyControl=new double[DOF+1];
            if(Rand_noise==null)
                Rand_noise=new Random();
            double sum2 = 0;
            for (int i = 0; i < DOF; ++i)
            {
                double value = Rand_noise.NextDouble() * (high[i] - low[i]) + low[i];
                noisyControl[i] = value;
                sum2 += value * value;
            }
            double oneOverLength = amplitude_noise / Math.Sqrt(sum2);
            for (int i = 0; i < DOF; ++i)
                noisyControl[i] *= oneOverLength;
            noisyControl[DOF] = Rand_noise.NextDouble() * duration_max - duration_max * 0.5;
        }
    }
}
